'''
使用ROI分五个区识别黑线，根据Otus动态获取阈值
通过移动平均滤波进行滤波检测
'''
import sensor
import display,time
from pyb import LED,Timer,UART
from ucollections import deque   # deque双端队列类，快速进行增删改查
#************************************外设*************************************
sensor.reset()                       # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE)  # or sensor.GRAYSCALE
sensor.set_framesize(sensor.QQVGA)  # Special 128x160 framesize for LCD Shield.
lcd       = display.SPIDisplay()     # Initialize the lcd screen.
red_led   = LED(1)
green_led = LED(2)
blue_led  = LED(3)
red_led.off()
green_led.off()
blue_led.off()
uart      = UART(3,115200)
uart.init(115200,bits=8,parity=None,stop=1)
#************************************变量*************************************
red_point = [0,0,0,0,0]
matrix = [[0 for _ in range(6)] for _ in range(5)]     # 创建一个二维矩阵
red_threshold = [(128,255)] # 像素阈值（128-255）就是白色区域二值化只有0和255
size = 160//16              # 图像宽度并分区
height_1 = 20
height_2 = 40
track_roi = [(size*0,height_1,size*3,height_2),
             (size*3,height_1,size*3,height_2),
             (size*6,height_1,size*4,height_2),
             (size*10,height_1,size*3,height_2),
             (size*13,height_1,size*3,height_2)]

# 计算误差
def Error_calculate(data):
    if data[1] == 0 and data[2] == 1 and data[3] == 0:  # 直行
        return 0
    elif data[1] == 1 and data[2] == 1 and data[3] == 0:
        return 1
    elif data[1] == 1 and data[2] == 0 and data[3] == 0:
        return 2
    elif data[1] == 0 and data[2] == 1 and data[3] == 1:
        return 3    # -0.75
    elif data[1] == 0 and data[2] == 0 and data[3] == 1:
        return 4    # -1.5
    elif data[0] == 1 and data[1] == 0 and data[2] == 0 and data[3] == 0 and data[4] == 0:
        return 5    # 2
    elif data[0] == 0 and data[1] == 0 and data[2] == 0 and data[3] == 0 and data[4] == 1:
        return 6    # -2
    elif data[0] == 1 and data[1] == 1 and data[2] == 1 and data[3] == 1 and data[4] == 1:  # 路口
        return 7    # 3
    elif (data[0] == 1 or data[4] == 1) and (data[1] == 1 or data[2] == 1 or data[3] == 1):  # 路口
        return 7
    else:
        return 0

# 串口发送
def Uart_send(data):
    global uart
    send_data = [0x0a,0x0b]
    send_data.append(data)
    send_data.append(0x0c)
    uart.write(bytearray(send_data))
#    print(send_data)

# 移动平均滤波
def Moving_Average_Filter(r,mark):
    global matrix,red_point
    for m in range(len(matrix[r])-1,0,-1):
        matrix[r][m] = matrix[r][m-1]
    matrix[r][0] = mark
    if (sum(matrix[r])/len(matrix[r])) > 0.5:
        red_point[r] = 1
    else:
        red_point[r] = 0

# 找最大色块索引
def Find_Max_Blob(blobs):
    MaxBlobArea = 0
    MaxBlobIndex = 0
    if len(blobs) == 1:
        return 0
    else:
        for index in range(0,len(blobs)):
            if blobs[index].area() > MaxBlobArea:
                MaxBlobArea = blobs[index].area()
                MaxBlobIndex = index
        return MaxBlobIndex
# 找块
def Find_line(img):
    global red_threshold,track_roi
    for r in range(0,len(track_roi)):
        blobs = img.find_blobs(red_threshold,roi=track_roi[r],area_threshold=300)
        if blobs:
            for blob in blobs:
                img.draw_rectangle(blob.rect(),color=127)
            blob = blobs[Find_Max_Blob(blobs)]
            if r != 2:
                if (blob.pixels()/blob.area()) > 0.6:
                    Moving_Average_Filter(r,1) # 进行移动平均滤波
                else:
                    Moving_Average_Filter(r,0)
            else:
                Moving_Average_Filter(r,1)
        else:
            Moving_Average_Filter(r,0)

# 画线
def Draw_line(img):
    global red_point,track_roi
    # 画ROI区域
    for t in range(0,len(track_roi)):
        img.draw_rectangle(track_roi[t])
    # 画识别结果
    for r in range(0,len(red_point)):
        if red_point[r] == 0:
            img.draw_rectangle(track_roi[r][0]+track_roi[r][2]//6,100,track_roi[r][2]//6*4,15,color=127,fill=False)
        else:
            img.draw_rectangle(track_roi[r][0]+track_roi[r][2]//6,100,track_roi[r][2]//6*4,15,color=127,fill=True)

while True:
    img = sensor.snapshot()
    histogram = img.get_histogram()         # 获取灰度直方图
    Thresholds = histogram.get_threshold()  # Otus算法获取动态阈值
    img.binary([(0, Thresholds.value())])   # 根据阈值二值化
    img.erode(2)                            # 腐蚀
    img.dilate(2)                           # 膨胀
    Find_line(img)                          # 找线
    Uart_send(Error_calculate(red_point))   # 串口发数据
    Draw_line(img)                          # 画线
    lcd.write(img)
